#include <math.h> /* OJO: compilar con -lm */
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include "robot.h"

/* Factores de conversión entre grados y radianes */
#define G_TO_RAD M_PI/180.0
#define RAD_TO_G 180.0/M_PI


frame* UTOI(vec3 uform)
{
    /* IMPORTANTE: el llamador de la función debe liberar la memoria asignada
     * mediante gsl_matrix_free
     */
    double theta_grad=uform[2];
    double theta_rad=.0;
    frame *matriz;
    double cosn=.0, sen=.0;
    
    /* Convertimos el ángulo a radianes */
    theta_rad = theta_grad*G_TO_RAD;
    cosn = cos(theta_rad);
    sen = sin(theta_rad);
    
    matriz = gsl_matrix_alloc(3,3);
    assert(matriz != NULL);
    
    /* Seteamos los valores de la matriz */
    gsl_matrix_set(matriz,0,0,cosn);
    gsl_matrix_set(matriz,0,1,-sen);
    gsl_matrix_set(matriz,0,2,uform[0]);
    gsl_matrix_set(matriz,1,0,sen);
    gsl_matrix_set(matriz,1,1,cosn);
    gsl_matrix_set(matriz,1,2,uform[1]);
    gsl_matrix_set(matriz,2,0,0.0);
    gsl_matrix_set(matriz,2,1,0.0);
    gsl_matrix_set(matriz,2,2,1.0);
    
    return matriz;
}

void ITOU(frame *iform, vec3 uform)
{
    double a21=.0, a22=.0;
    double theta_rad=.0, theta_grad=.0;
    
    a21 = gsl_matrix_get(iform,1,0);
    a22 = gsl_matrix_get(iform,1,1);
    theta_rad = atan2(a21, a22);
    theta_grad = theta_rad*RAD_TO_G;
    
    /* Seteamos apropiadamente los campos de uform */
    uform[0] = gsl_matrix_get(iform,0,2);
    uform[1] = gsl_matrix_get(iform,1,2);
    uform[2] = theta_grad;
}

frame* TMULT(frame *brela, frame *crelb)
{
    /* IMPORTANTE: el llamador de la función debe liberar la memoria asignada
     * mediante gsl_matrix_free
     */
    frame *res;
    
    /* Asignamos memoria para la matriz a devolver */
    res = gsl_matrix_alloc(3,3);

    /* Efectuamos la multiplicación */
    mul_matrix(brela, crelb, res);
    return res;
}

frame* TINVERT(frame *brela)
{
    /* IMPORTANTE: el llamador de la función debe liberar la memoria asignada
     * mediante gsl_matrix_free
     */
    frame *res;
    double a11=.0, a12=.0, a13=.0, a21=.0, a22=.0, a23=.0;
    
    /* Extraemos los valores de la matriz de entrada */
    a11 = gsl_matrix_get(brela,0,0);
    a12 = gsl_matrix_get(brela,0,1);
    a13 = gsl_matrix_get(brela,0,2);
    a21 = gsl_matrix_get(brela,1,0);
    a22 = gsl_matrix_get(brela,1,1);
    a23 = gsl_matrix_get(brela,1,2);
    
    /* Reservamos memoria para la matriz resultado */
    res = gsl_matrix_alloc(3,3);
    
    /* Seteamos los valores de la matriz resultado apropiadamente */
    gsl_matrix_set(res,0,0,a11);
    gsl_matrix_set(res,0,1,a21);
    gsl_matrix_set(res,0,2,-(a11*a13+a21*a23));
    gsl_matrix_set(res,1,0,a12);
    gsl_matrix_set(res,1,1,a22);
    gsl_matrix_set(res,1,2,-(a12*a13+a22*a23));
    gsl_matrix_set(res,2,0,0.0);
    gsl_matrix_set(res,2,1,0.0);
    gsl_matrix_set(res,2,2,1.0);
    return res;
}    

void mul_matrix(frame *m1, frame *m2, frame *res)
{
    int i=0, j=0, k=0;
    double sum=.0, tmp1=.0, tmp2=.0;
    
    for (i=0; i<3; i++)
    {
        for (j=0; j<3; j++)
        {
            sum = .0;
            for (k=0; k<3; k++)
            {
                tmp1 = gsl_matrix_get(m1,i,k);
                tmp2 = gsl_matrix_get(m2,k,j);
                sum += tmp1*tmp2;
            }
            gsl_matrix_set(res,i,j,sum);
        }
    }
}

frame* KIN(vec3 theta)
{
    /* IMPORTANTE: el llamador de la función debe liberar la memoria asignada
     * mediante gsl_matrix_free
     */
    frame *res;
    double alfa=.0, beta=.0, gamma=0.;
    
    gamma = theta[0]*G_TO_RAD;
    alfa = (theta[0]+theta[1])*G_TO_RAD;
    beta = (theta[0]+theta[1]+theta[2])*G_TO_RAD;
    
    /* Reservamos memoria para la matriz a devolver */
    res = gsl_matrix_alloc(3,3);
    
    /* La seteamos como corresponde */
    gsl_matrix_set(res,0,0,cos(beta));
    gsl_matrix_set(res,0,1,-sin(beta));
    gsl_matrix_set(res,0,2,L1*cos(gamma)+L2*cos(alfa));
    gsl_matrix_set(res,1,0, sin(beta));
    gsl_matrix_set(res,1,1,cos(beta));
    gsl_matrix_set(res,1,2,L1*sin(gamma)+L2*sin(alfa));
    gsl_matrix_set(res,2,0,0.0);
    gsl_matrix_set(res,2,1,0.0);
    gsl_matrix_set(res,2,2,1.0);
    
    return res;
}

frame *WHERE(vec3 theta, frame *trelw, frame *srelb)
{
    /* IMPORTANTE: el llamador de la función debe liberar la memoria asignada
     * mediante gsl_matrix_free
     */
    frame *trels; /* output */
    
    /* Los siguientes frames son para guardar cálculos intermedios */
    frame *inv;
    frame *tmp;
    frame *wrelb;
    
    wrelb = KIN(theta);
    /* printf("Imprimiendo wrelb en forma matricial:\n"); */
    /* print_frame(wrelb, stdout); */
    inv = TINVERT(srelb);
    tmp = TMULT(inv,wrelb);
    trels = TMULT(tmp, trelw);
    
    /* Liberamos la memoria de las matrices intermedias */
    gsl_matrix_free(wrelb);
    gsl_matrix_free(inv);
    gsl_matrix_free(tmp);
    wrelb = NULL;
    inv = NULL;
    tmp = NULL;
    
    return trels;
}

void print_frame(frame *fr, FILE *f)
{
    int i=0, j=0;
    
    assert (fr!=NULL && f!=NULL);
    for (i=0; i<3; i++)
    {
        for (j=0; j<3; j++)
            fprintf(f, "%f\t", gsl_matrix_get(fr,i,j));
        fprintf(f, "\n");
    }
    fprintf(f, "\n\n");
}

void print_vec3(vec3 v, FILE *f)
{
    int i=0;
    for (i=0; i<3; i++)
        fprintf(f, "%f ", v[i]);
    fprintf(f, "\n");
}
    
Bool INVKIN(frame *wrelb, vec3 current, vec3 near, vec3 far)
{
    double x=.0, y=.0, theta=.0;
    double c2=.0, beta=.0, cfi=.0, fi=.0, theta2=.0, theta1=.0;
    double theta1_inv=.0, theta3=.0, theta3_inv=.0, theta2_inv=.0;
    double theta1_c=.0, theta2_c=.0, theta3_c=.0;
    double xc=.0, yc=.0, xs=.0, ys=.0, d1=.0, d2=.0;
    vec3 aux;
        
    /* Extraemos los valores de x,y & theta */
    ITOU(wrelb, aux);
    x = aux[0];
    y = aux[1];
    theta = aux[2]*G_TO_RAD; /* aux[2] está en grados */
    
    c2 = x*x+y*y-L1*L1-L2*L2;
    c2 /= (2*L1*L2);

    if (fabs(c2) > 1)
        return FALSE;
    
    theta2 = -acos(c2); /* theta2 está en radianes */
    theta2_inv = -theta2;
    if (theta2 < THETA_MIN)
        return FALSE;
    
    beta = atan2(y,x);
    cfi = x*x+y*y+L1*L1-L2*L2;
    cfi /= (2.0*L1*sqrt(x*x+y*y));
    fi = acos(cfi);
    
    theta1 = beta+fi;
    theta1_inv = beta-fi;
    if (theta1 > M_PI)
        theta1 -= (2.0*M_PI);
    if (theta1_inv < -M_PI)
        theta1_inv += (2.0*M_PI);
    
    if ( (fabs(theta1) > THETA_MAX)&&(fabs(theta1_inv) > THETA_MAX) )
        return FALSE;
    
    theta3 = theta-theta1-theta2;
    theta3_inv = theta-theta1_inv-theta2_inv;
    
    if (theta3 > M_PI)
        theta3 -= (2.0*M_PI);
    else if (theta3 < -M_PI)
        theta3 += (2.0*M_PI);
    
    if (theta3_inv > M_PI)
        theta3_inv -= (2.0*M_PI);
    else if (theta3_inv < -M_PI)
        theta3_inv += (2.0*M_PI);
    
    if ( (fabs(theta3) > THETA_MAX)&&(fabs(theta3_inv)  > THETA_MAX) )
        return FALSE;
    
    theta1_c = current[0];
    theta2_c = current[1];
    theta3_c = current[2];
    
    /* Están en grados, para usar las funciones de C los pasamos a radianes */
    theta1_c *= G_TO_RAD;
    theta2_c *= G_TO_RAD;
    theta3_c *= G_TO_RAD;
    
    xc = cos(theta1_c);
    yc = sin(theta1_c);
    xs = cos(theta1);
    ys = sin(theta1);
    d1 = (xc-xs)*(xc-xs)+(yc-ys)*(yc-ys);
    xs = cos(theta1_inv);
    ys = sin(theta1_inv);
    d2 = (xc-xs)*(xc-xs)+(yc-ys)*(yc-ys);

    /* si llegamos hasta aca es que al menos hay una solucion. Ahora hay que
     verificar si las dos soluciones o solo una de ellas corresponde a 
     angulos de articulacion comprendidos en el rango (THETA_MIN,THETA_MAX) */

    if ( (fabs(theta1)>THETA_MAX) || (fabs(theta2)>THETA_MAX) || 
	 (fabs(theta3)>THETA_MAX) )  
    /* la unica solucion es theta#_inv */
    {
        /* Los ángulos están en radianes => los pasamos a grados */
        near[0] = theta1_inv*RAD_TO_G;
        near[1] = theta2_inv*RAD_TO_G;
        near[2] = theta3_inv*RAD_TO_G;
	far[0] = near[0];
	far[1] = near[1];
	far[2] = near[2]; 
	printf("hay una sola solucion: 1\n");
    }
    else if ( (fabs(theta1_inv)>THETA_MAX) || (fabs(theta2_inv)>THETA_MAX) || 
	 (fabs(theta3_inv)>THETA_MAX) )  
    /* la unica solucion es theta# */
    {
        near[0] = theta1*RAD_TO_G;
        near[1] = theta2*RAD_TO_G;
        near[2] = theta3*RAD_TO_G;
	far[0] = near[0];
	far[1] = near[1];
	far[2] = near[2]; 
	printf("hay una sola solucion: 2\n");
    }
    else if (d1 < d2)
    {
        near[0] = theta1*RAD_TO_G;
        near[1] = theta2*RAD_TO_G;
        near[2] = theta3*RAD_TO_G;
        far[0] = theta1_inv*RAD_TO_G;
        far[1] = theta2_inv*RAD_TO_G;
        far[2] = theta3_inv*RAD_TO_G;
    }
    else
    {
        near[0] = theta1_inv*RAD_TO_G;
        near[1] = theta2_inv*RAD_TO_G;
        near[2] = theta3_inv*RAD_TO_G;
        far[0] = theta1*RAD_TO_G;
        far[1] = theta2*RAD_TO_G;
        far[2] = theta3*RAD_TO_G;
    }
    /*printf("near: %6.2f %6.2f %6.2f\n",near[0],near[1],near[2]);
    printf("far: %6.2f %6.2f %6.2f\n",far[0],far[1],far[2]);*/
    return TRUE;
}
    
Bool SOLVE(frame *trels, frame *trelw, frame *srelb, vec3 current, vec3 near, vec3 far)
{
    frame *tmp1;
    frame *tmp2;
    frame *wrelb;
    Bool sol = FALSE;
    
    tmp1 = TMULT(srelb, trels);
    tmp2 = TINVERT(trelw);
    wrelb = TMULT(tmp1, tmp2);
    sol = INVKIN(wrelb, current, near, far);
    /* Liberamos la memoria asignada a los frames temporales */
    gsl_matrix_free(tmp1);
    gsl_matrix_free(tmp2);
    gsl_matrix_free(wrelb);
    tmp1 = NULL;
    tmp2 = NULL;
    wrelb = NULL;    
    return sol;
}
    